#include <stdio.h>
#include "driver/ledc.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#define SERVO_GPIO 13
#define LEDC_TIMER LEDC_TIMER_0
#define LEDC_CHANNEL LEDC_CHANNEL_0
#define LEDC_MODE LEDC_LOW_SPEED_MODE
#define LEDC_FREQUENCY 50
#define LEDC_DUTY_RES LEDC_TIMER_13_BIT
#define SERVO_MIN_PULSEWIDTH 500
#define SERVO_MAX_PULSEWIDTH 2500
#define SERVO_MAX_DEGREE 180

static const char *TAG = "Servo";

void app_main(void) {
    ledc_timer_config_t ledc_timer = {
        .speed_mode = LEDC_MODE,
        .timer_num = LEDC_TIMER,
        .duty_resolution = LEDC_DUTY_RES,
        .freq_hz = LEDC_FREQUENCY,
        .clk_cfg = LEDC_AUTO_CLK,
    };
    ledc_timer_config(&ledc_timer);

    ledc_channel_config_t ledc_channel = {
        .channel = LEDC_CHANNEL,
        .duty = 0,
        .gpio_num = SERVO_GPIO,
        .speed_mode = LEDC_MODE,
        .timer_sel = LEDC_TIMER,
        .hpoint = 0,
    };
    ledc_channel_config(&ledc_channel);

    void set_servo_angle(int angle) {
        if (angle < 0) angle = 0;
        if (angle > SERVO_MAX_DEGREE) angle = SERVO_MAX_DEGREE;

        uint32_t pulsewidth = (SERVO_MIN_PULSEWIDTH + (angle * (SERVO_MAX_PULSEWIDTH - SERVO_MIN_PULSEWIDTH) / SERVO_MAX_DEGREE));
        uint32_t duty = (pulsewidth * (1 << LEDC_DUTY_RES)) / 20000;

        ledc_set_duty(LEDC_MODE, LEDC_CHANNEL, duty);
        ledc_update_duty(LEDC_MODE, LEDC_CHANNEL);
    }

    while (1) {
        for (int angle = 0; angle <= 180; angle += 10) {
            set_servo_angle(angle);
            vTaskDelay(500 / portTICK_PERIOD_MS);
        }
        for (int angle = 180; angle >= 0; angle -= 10) {
            set_servo_angle(angle);
            vTaskDelay(500 / portTICK_PERIOD_MS);
        }
    }
}
